struct HeapFreeBlock {
    unsigned long size;
    struct HeapFreeBlock* next;
};

struct HeapFreeBlock* HeapFreeBlockHead = 0;

void YemuMallocInit(unsigned long heap_start, unsigned long heap_end) {
    YemuPrint("Heap : [0x%lx - 0x%lx)\n", heap_start, heap_end);
    HeapFreeBlockHead = (struct HeapFreeBlock*)heap_start;
    HeapFreeBlockHead->size = heap_end - heap_start - sizeof(struct HeapFreeBlock);
    HeapFreeBlockHead->next = 0;
    return ;
}

void* YemuMalloc(unsigned int size) {
    size = (size + 7) & ~7;

    struct HeapFreeBlock* prev = 0;
    struct HeapFreeBlock* curr = HeapFreeBlockHead;

    while (curr != 0) {
        if (curr->size >= size) {
            if (curr->size >= size + sizeof(struct HeapFreeBlock) + 8) {
                
                unsigned long new_block_addr = (unsigned long)curr + sizeof(struct HeapFreeBlock) + size;
                struct HeapFreeBlock* new_block = (struct HeapFreeBlock*)new_block_addr;
                new_block->size = curr->size - size - sizeof(struct HeapFreeBlock);
                new_block->next = curr->next;

                curr->size = size;
                if (prev) prev->next = new_block;
                else HeapFreeBlockHead = new_block;
                return (void*)((unsigned long)curr + sizeof(struct HeapFreeBlock));
            } else {
                if (prev) prev->next = curr->next;
                else HeapFreeBlockHead = curr->next;
                return (void*)((unsigned long)curr + sizeof(struct HeapFreeBlock));
            }
        }
        prev = curr;
        curr = curr->next;
    }
    
    return 0;
}

void YemuFree(void* ptr) {
    if (!ptr) return;

    struct HeapFreeBlock* block = (struct HeapFreeBlock*)((unsigned long)ptr - sizeof(struct HeapFreeBlock));
    struct HeapFreeBlock* prev = 0;
    struct HeapFreeBlock* curr = HeapFreeBlockHead;

    while (curr != 0 && curr < block) {
        prev = curr;
        curr = curr->next;
    }
    block->next = curr;
    if (prev) prev->next = block;
    else HeapFreeBlockHead = block;

    if (prev != 0 && (unsigned long)prev + sizeof(struct HeapFreeBlock) + prev->size == (unsigned long)block) {
        prev->size += sizeof(struct HeapFreeBlock) + block->size;
        prev->next = block->next;
        block = prev;
    }
    
    if (curr != 0 && (unsigned long)block + sizeof(struct HeapFreeBlock) + block->size == (unsigned long)curr) {
        block->size += sizeof(struct HeapFreeBlock) + curr->size;
        block->next = curr->next;
    }

    return ;
}
